﻿import time
import cv2 
import global_var as glv
import visual_function as vf
import sys
sys.path.append("../")
import Serial_Servo_Running as SSR
import PWMServo


def main_init(PWM_Y):
    PWMServo.setServo(1, PWM_Y, 500)
    PWMServo.setServo(2, 1500, 500)
    time.sleep(0.5)

def go_oneStep():
    SSR.running_action_group(glv.actionGroup["go_straight"], 1)
class basic_game():
    def __init__(self,running = False):
        self.actionGroup = glv.actionGroup
        self.resize_width = glv.getVal("resize_width")
        self.resize_height = glv.getVal("resize_height")
        ###变量###
        self.running = running #是否允许舵机运动
        self.state = None #动作执行种类
        self.get_line = False #是否能获得黑色车道线
        self.lines = None #检测车道线
        ###云台阈值###
        self.PWM_Y = 1700 #默认先抬头看
        self.PWM_MaxY = 2300 #默认角度最大数值
        self.PWM_H_UpY = 1700 #抬头调位姿云台角度
        self.PWM_H_DownY = 2200 #低头调位姿云台角度
        PWMServo.setServo(1, self.PWM_Y, 500)
        PWMServo.setServo(2, 1500, 500)
        time.sleep(0.5)
        ###线中心区域边界###
        self.line_minCenter = self.resize_width/2 - self.resize_width/8 #中心区域左边界
        self.line_maxCenter = self.resize_width/2 + self.resize_width/8 #中心区域右边界
        ###标志位###
        self.headUp = True #抬头调整位姿-大
        self.headDown = False #抬头调整位姿-小
        self.get_line = False #是否检测到车道线
    def execute(self,PWM_Y = 1500, PWM_X = 1500, executeNum = 1, extraTime = 0.05):
        if self.state is not None:
            print(self.state)
            if self.state == "change_PWM":
                PWMServo.setServo(1, PWM_Y, 500) #Y轴云台
                time.sleep(0.5)
            elif self.state == "change_PWM_X":
                PWMServo.setServo(2, PWM_X, 500) #X轴云台
                time.sleep(0.5)
            else:
                if self.running:
                    SSR.running_action_group(self.actionGroup[self.state], executeNum)
            self.state = None
            time.sleep(extraTime)
        else:
            return
    def logic_position_up(self):
        if self.get_line:
            self.get_line = False
            if self.lines["left"] is not None and self.lines["right"] is not None:
                line_center = (self.lines["left"]["b"] - self.lines["right"]["b"])/(self.lines["right"]["k"] - self.lines["left"]['k'])
                if line_center > self.line_minCenter and line_center < self.line_maxCenter:
                    self.state = "change_PWM"
                    basic_game.execute(self,self.PWM_H_DownY)
                    self.headUp = False
                    self.headDown = True
                elif line_center < self.line_minCenter:
                    self.state = "turn_left"
                    basic_game.execute(self)
                elif line_center > self.line_maxCenter:
                    self.state = "turn_right"
                    basic_game.execute(self)
            elif self.lines["left"] is not None:
                self.state = "turn_right"
                basic_game.execute(self)
            elif self.lines["right"] is not None:
                self.state = "turn_left"
                basic_game.execute(self)
            else:
                self.state = "change_PWM"
                basic_game.execute(self, self.PWM_H_DownY)
                self.headUp = False
                self.headDown = True
        else:
            self.state = "change_PWM"
            basic_game.execute(self, self.PWM_H_DownY)
            self.headUp = False
            self.headDown = True  
    def logic_position_down(self):
        #看脚边黑线，能看到说明已经逼近赛道边缘，需要做急救措施
        if self.get_line:
            self.get_line = False
            if self.lines["left"] is not None and self.lines["right"] is not None:
                line_center = (self.lines["left"]["b"] - self.lines["right"]["b"])/(self.lines["right"]["k"] - self.lines["left"]['k'])
                if line_center > self.line_minCenter and line_center < self.line_maxCenter:
                    self.state = "change_PWM"
                    basic_game.execute(self, self.PWM_Y)
                    self.headDown = False
                elif line_center < self.line_minCenter:
                    self.state = "turn_left"
                    basic_game.execute(self)
                elif line_center > self.line_maxCenter:
                    self.state = "turn_right"
                    basic_game.execute(self)
            elif self.lines["left"] is not None:
                if self.lines["left"]["a"] <= -55:
                    self.state = "turnRight_up55"
                elif self.lines["left"]["a"] > -55 and self.lines["left"]["a"] < -40:
                    self.state = "turnRight_40to55"
                elif self.lines["left"]["a"] >= -40:
                    self.state = "turnRight_down40"
                basic_game.execute(self)
            elif self.lines["right"] is not None:
                if self.lines["right"]["a"] >= 55:
                    self.state = "turnLeft_up55"
                elif self.lines["right"]["a"] < 55 and self.lines["right"]["a"] > 40:
                    self.state = "turnLeft_40to55"
                elif self.lines["right"]["a"] <= 40:
                    self.state = "turnLeft_down40"
                basic_game.execute(self)

            else:
                self.state = "change_PWM"
                basic_game.execute(self, self.PWM_Y)
                self.headDown = False
        else:
            self.state = "change_PWM"
            basic_game.execute(self, self.PWM_Y)
            self.headDown = False

class NextColorGate(basic_game):
    def __init__(self, running = False):
        basic_game.__init__(self, running)
        self.PWM_Y == 2000
        self.mission = None
    def logic_getColor(self):
        if self.color is None:
            self.state = "go_straight"
            basic_game.execute(self)
        else:
            self.mission = glv.colorMission[self.color]

    def run(self, hsvImage, outImage):
        if self.headUp:
            self.get_line, self.lines = vf.getLineByColor(hsvImage, 'black',outImage = outImage)
            if outImage is not None:
                cv2.imshow("stepframe",outImage)
                cv2.waitKey(1)
            basic_game.logic_position_up(self)
        elif self.headDown:
            self.get_line, self.lines = vf.getLineByColor(hsvImage, 'black',outImage = outImage)
            if outImage is not None:
                cv2.imshow("stepframe",outImage)
                cv2.waitKey(1)
            basic_game.logic_position_down(self)
        else:
            self.color = vf.getColor(hsvImage,2000)
            NextColorGate.logic_getColor(self)
            if self.mission is not None:
                return True, self.mission
        return False, None
            
